package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.Talon;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.templates.RobotMap;

/**
 *
 * @author Super NURDs
 */

public class Hopper extends Subsystem {
    // Put methods for controlling this subsystem
    // here. Call these from Commands.

    Talon hopper;
    private Relay hopperLightingRelay;
    private LimitSwitch startLimitSwitch;
    private LimitSwitch stopLimitSwitch;

    public boolean isLightsOn;
    
    private static final int POLARITY = 1;
    
    public Hopper(){
        super("Hopper");
        try {
            hopper = new Talon(RobotMap.HOPPER_TALON);
            hopper.setSafetyEnabled(false);
       
            hopperLightingRelay = new Relay(RobotMap.HOPPER_LIGHTING_RELAY, Relay.Direction.kForward);
            startLimitSwitch = new LimitSwitch(RobotMap.HOPPER_START_SWITCH);
            stopLimitSwitch = new LimitSwitch(RobotMap.HOPPER_STOP_SWITCH);
        }
        catch (Exception e){
            e.printStackTrace();
        }
    }
    
    public void initDefaultCommand() {  
    }
    
    public void setSpeed(double speed){
        try {
            double hopperSpeed = (POLARITY*speed);
            hopper.set(hopperSpeed);
        }
        
        catch (Exception e){
            e.printStackTrace();
        }     
    }
    
    public double getSpeed() {
        return hopper.get();
    }
    
    public boolean getStartSwitchState() {
        return !startLimitSwitch.isOpen();
    }

    public boolean getStopSwitchState() {
        return !stopLimitSwitch.isOpen();
    }
    

    public void hopperLightsOn() {
        hopperLightingRelay.set(Relay.Value.kOn);
        isLightsOn = true;
    }
    
    public void hopperLightsOff() {
        hopperLightingRelay.set(Relay.Value.kOff);
        isLightsOn = false;
    }
    
    public boolean isHopperLightsOn() {
        return isLightsOn;
    }

  }